import math
import numpy as np


def smoothing_factor(t_e, cutoff):
    r = 2 * math.pi * cutoff * t_e
    return r / (r + 1)


def exponential_smoothing(a, x, x_prev):
    return a * x + (1 - a) * x_prev


class OneEuroFilter:
    def __init__(self, x0, dx0=0.0, min_cutoff=1.0, beta=0.0,
                 d_cutoff=1.0):
        """
        min_cutoff最小截止频率：减小可以降低慢速抖动
        beta：速度系数，增加可以减少滞后
        :param t0:
        :param x0:
        :param dx0:
        :param min_cutoff:
        :param beta:
        :param d_cutoff:
        """
        """Initialize the one euro filter."""
        # The parameters.
        self.min_cutoff = float(min_cutoff)
        self.beta = float(beta)
        self.d_cutoff = float(d_cutoff)
        # Previous values.
        self.x_prev = float(x0)
        self.dx_prev = float(dx0)
        # self.t_prev = float(t0)

    def __call__(self, x):
        """
        分别对信号的变化率和信号进行了指数平滑
        :param t:
        :param x:
        :return:
        """

        """Compute the filtered signal."""
        # t_e = t - self.t_prev
        t_e = 0.033

        # The filtered derivative of the signal.
        a_d = smoothing_factor(t_e, self.d_cutoff)  # 计算alpha
        dx = (x - self.x_prev) / t_e  # 导数即变化率/速度
        dx_hat = exponential_smoothing(a_d, dx, self.dx_prev)  # 对变化率进行指数平滑

        # The filtered signal.
        cutoff = self.min_cutoff + self.beta * abs(dx_hat)  # 截止频率fc
        a = smoothing_factor(t_e, cutoff)  # alpha
        x_hat = exponential_smoothing(a, x, self.x_prev)  # 滤波后的值

        # Memorize the previous values.
        self.x_prev = x_hat
        self.dx_prev = dx_hat
        # self.t_prev = t

        return x_hat


class Filter3D():
    def __init__(self, dx=0, min_cutoff=1.0, beta=0.5,
                 d_cutoff=1.0, num=32):
        self.num = num
        self.filter = []
        self.dx = dx
        self.min_cutoff = min_cutoff
        self.beta = beta
        self.d_cutoff = d_cutoff

    def run(self, points, camnum=0):
        if camnum == 0:
            self.filter = []
            for i in range(len(points)):
                self.filter.append([OneEuroFilter(
                    points[i][0], dx0=self.dx, min_cutoff=self.min_cutoff, beta=self.beta, d_cutoff=self.d_cutoff),
                                    OneEuroFilter(points[i][
                                                      1], dx0=self.dx, min_cutoff=self.min_cutoff, beta=self.beta, d_cutoff=self.d_cutoff),
                                    OneEuroFilter(points[i][
                                                      2], dx0=self.dx, min_cutoff=self.min_cutoff, beta=self.beta, d_cutoff=self.d_cutoff)])
            return points
        points_filter = []
        for i in range(len(points)):
            points_filter.append([self.filter[i][0](points[i][0]),
                                  self.filter[i][1](points[i][1]),
                                  self.filter[i][2](points[i][2])])
        return np.array(points_filter)
